/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.ADXL345_I2C;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.RobotMap;
import edu.wpi.first.wpilibj.templates.commands.DriveWithJoystick;

/**
 *
 * @author Wendy Chan
 */
public class DriveTrain extends PIDSubsystem {
    private static final double Kp = 2.0;
    private static final double Ki = 0.0;
    private static final double Kd = 0.0;

    private RobotDrive PIDdrive = new RobotDrive(RobotMap.frontLeftMotor, RobotMap.rearLeftMotor, RobotMap.frontRightMotor, RobotMap.rearRightMotor);
    private Gyro gyro;
    private ADXL345_I2C accel;
    private Encoder encoderLeft;
    private Encoder encoderRight;

    private double angle;
    private double accread;
    private double tilt;
    private double k;

    // Initialize your subsystem here
    public DriveTrain() {
        super("DriveTrain", Kp, Ki, Kd);

        // Use these to get going:
        // setSetpoint() -  Sets where the PID controller should move the system
        //                  to
        // enable() - Enables the PID controller.
        
        gyro = new Gyro(RobotMap.gyrochan);
        //accel = new Accelerometer(RobotMap.accelchan);
        accel = new ADXL345_I2C (1, ADXL345_I2C.DataFormat_Range.k4G);
        encoderRight = new Encoder(RobotMap.encoderRightA,RobotMap.encoderRightB);
        encoderLeft = new Encoder(RobotMap.encoderLeftA,RobotMap.encoderLeftB);
        PIDdrive.setSafetyEnabled(false);
        k = 1;

    }
    
    public void initDefaultCommand() {
        // Set the default command for a subsystem here.
        //setDefaultCommand(new MySpecialCommand());
        setDefaultCommand(new DriveWithJoystick());
    }

    protected double returnPIDInput() {
        // Return your input value for the PID loop
        // e.g. a sensor, like a potentiometer:
        // yourPot.getAverageVoltage() / kYourMaxVoltage;

        /*
        accread = accel.getAcceleration(ADXL345_I2C.Axes.kX);
        System.out.println("Acceleration (X axis)= " + accread);
        accread = accel.getAcceleration(ADXL345_I2C.Axes.kY);
        System.out.println("Acceleration (Y axis)= " + accread);
        accread = accel.getAcceleration(ADXL345_I2C.Axes.kZ);
        System.out.println("Acceleration (Z axis)= " + accread);
        //calculate tilt a.k.a. angle of inclination
        tilt = accread * k * 180.0 / 3.1416;
        System.out.println("Tilt Angle = " + tilt + " degrees");
         *
         */
        tilt = readtilt();
        return tilt;
    }

    protected void usePIDOutput(double output) {
        // Use output to drive your system, like a motor
        // e.g. yourMotor.set(output);
        balance(tilt);
    }

    public void driveWithJoy(double y, double twist){
        // Remove sensitivity of the joystick
        if (y < 0.25 && y > -0.25){
            y = 0;
        }
        /*  Change this to a logarithmic adjustment
        if (twist < 0.3 && twist > -0.3){
            twist = 0;
        }
         */
        PIDdrive.arcadeDrive(-y, -twist);  //forward on our robot is negative
        System.out.println("Drive Me");
    }

    public void zerogyro(){
        gyro.reset();
    }

    public void readgyro(){
        angle = gyro.getAngle();
        System.out.println("Gyro Angle = " + angle);
    }

    public double readtilt(){
        accread = accel.getAcceleration(ADXL345_I2C.Axes.kX);
        System.out.println("Acceleration (X axis)= " + accread);
        accread = accel.getAcceleration(ADXL345_I2C.Axes.kY);
        System.out.println("Acceleration (Y axis)= " + accread);
        accread = accel.getAcceleration(ADXL345_I2C.Axes.kZ);
        System.out.println("Acceleration (Z axis)= " + accread);
        //calculate tilt a.k.a. angle of inclination
        tilt = accread * k * 180.0 / 3.1416;
        System.out.println("Tilt Angle = " + tilt + " degrees");
        return tilt;
    }

    public void balance(double tilt){
        //Test this balancing drive algorithm after verifying and adjusting accelerometer readings
        if (tilt > 10){
            PIDdrive.arcadeDrive(-1.0, 0);
        }
        if (tilt <= 10 && tilt > 3.0){
            PIDdrive.arcadeDrive(-0.5, 0);
        }
        if (tilt < -3.0 && tilt >= -10.0){
            PIDdrive.arcadeDrive(0.5, 0);
        }
        if (tilt < -10.0){
            PIDdrive.arcadeDrive(1.0, 0);
        }
    }

    public void readencoders(){
        SmartDashboard.putDouble("Right Encoder Rate", encoderRight.getRate());
        SmartDashboard.putDouble("Left Encoder Rate", encoderLeft.getRate());
    }

}